#ifndef CV_BASE_HPP_
#define CV_BASE_HPP_

#include <stdio.h>
#include <stdexcept>
#include "opencv2/core/core.hpp"

namespace cvbase { 

template<typename T, size_t n>
class Coeff {
    size_t order_ = n;
    T coeff_[n+1]; // y = a0 + a1 * x + a2 * x^2 + ... + a_n-1 * x^(n-1)  
    void check_idx(size_t order_idx) const {
        if (order_idx > order_) { throw std::out_of_range("invalid access"); }
    }

public:
    const T& operator[](size_t order_idx) const { check_idx(order_idx); return coeff_[order_idx]; } 
    T& operator[](size_t order_idx) { check_idx(order_idx); return coeff_[order_idx]; } 
    
    friend std::ostream& operator<<(std::ostream& os, const Coeff& rhs){
        std::stringstream ss; 
        for (size_t order_idx = 0; order_idx <= n; ++order_idx) {
            ss << rhs.coeff_[order_idx];
            if (order_idx != n) { ss << ", "; }
        }
        os << ss.str();
        return os;
    }
};



template<typename T>
class Line {   
    typedef cv::Point_<T> Point;
    typedef cv::Rect_<T>  Rect;
    Point pts_[2];       
    T length_;
    T theta_;            // deg range [0-180) 
    Coeff<T, 1> coeff_;  // y = a0 + a1 * x

    void cal_coeff(const Point &diff) {
        if (diff.x == 0) {
            coeff_[1] = std::numeric_limits<T>::infinity();
            coeff_[0] = 0;
            return;
        }

        // y = b + ax
        T &&a = diff.y / diff.x;
        T &&b = pts_[0].y - a * pts_[0].x;
        coeff_[0] = b;
        coeff_[1] = a;
    }

    void check_idx(size_t order_idx) const {
        if (order_idx > 1) { throw std::out_of_range("invalid access"); }
    }

public:
    Line() {}
    Line(const cv::Point2f &p1, const cv::Point2f &p2) {
        pts_[0] = p1;
        pts_[1] = p2;
        length_ = euclideanDist(pts_[0], pts_[1]);
        Point diff = pts_[0] - pts_[1];
        cal_coeff(diff);
        theta_ = cv::fastAtan2(diff.y, diff.x); // deg range [0-360]
        theta_ -= 180;                          // shift to [-180-180]
        line_symmetric_angle(theta_);           // symmetric property of non-oriented line 
    }


    Point cal_intersection(const Line& rhs) const{
        T a1 = coeff_[1];     T b1 = coeff_[0]; 
        T a2 = rhs.coeff_[1]; T b2 = rhs.coeff_[0]; 

        if (std::isinf(a1) && std::isinf(a2)) {
            return Point(std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity());
        } else if (std::isinf(a1)) {
            return Point(pt(0).x, b2 + a2 * pt(0).x);
        } else if (std::isinf(a2)) {
            return Point(rhs.pt(0).x, b1 + a1 * rhs.pt(0).x);            
        }

        T x = (b2 - b1) / (a1 - a2);  
        T y = a1 * x + b1;
        return Point(x,y);
    }

    const Point& pt(size_t i) const { check_idx(i); return pts_[i]; }
    Point& pt(size_t i) { check_idx(i); return pts_[i]; }

    T length() const { return length_; }
    T theta()  const { return theta_; }
    const Coeff<T, 1>& coeff() const { return coeff_; };
    Coeff<T, 1>& coeff() { return coeff_; };
};

} // namespace cvbase

#endif // CV_BASE_HPP_